#include <math.h>
#include <assert.h>

#include "quadfish.h"
#include "QuadTree.h"
#include "FishThread.h"

//# of concurrent threads
int QuadTree::NumThreads = 0;
//total number of fish to simulate
int QuadTree::NumFish = 0;
//maximum number of fish in a leaf node
int QuadTree::MaxFish = 9;
double minFishPercent = .80;
//minimum number of fish underneath a tree node
int QuadTree::MinFish = QuadTree::MaxFish * minFishPercent;
//the root of the whole tree
QuadTree *QuadTree::Root = NULL;

void LeafNode::distributeFish( int &currentThread, int &fishOwned, FishThread threads[] )
{
    assert( currentThread <= NumThreads - 1 );

    owner = currentThread;
    fishOwned += contents.size();
    threads[currentThread].addLeaf( this );
    //if we're not on the last thread, and we've hit the minimum, go to next thread
    if ( fishOwned >= ceil(( double )NumFish / ( double )NumThreads ) && currentThread < NumThreads - 1 ) {
        currentThread++;
        fishOwned = 0;
    }
}

void LeafNode::intraQuadInteract()
{
    //zero out the stuff that is about to get accumulated into
    for ( FishList::iterator i = contents.begin(); i != contents.end();i++ ) {
        ( *i )->ax = ( *i )->ay = 0;
        ( *i )->v0x = ( *i )->v0y = 0;
        ( *i )->in_crowd = 0;
        ( *i )->v2x = ( *i )->v2y = 0;
        ( *i )->cogx = ( *i )->cogy = 0;
        ( *i )->in_sight = 0;
    }

    //perform all pair-wise interactions, on both a & b at the same time
    for ( FishList::iterator i = contents.begin(); i != contents.end();i++ ) {
        FishList::iterator j = i;
        for ( j++;
                j != contents.end();
                j++ ) {
            fish_t &a = **i;
            fish_t &b = **j;
            double dx = b.x - a.x;
            if ( fabs( dx ) > WALL_SEP - R_SIGHT ) {
                if ( dx < 0 ) {
                    dx = fabs( dx ) - WALL_SEP;
                } else {
                    dx = -fabs( dx ) + WALL_SEP;
                }
            }
            double dy = b.y - a.y;
            if ( fabs( dy ) > WALL_SEP - R_SIGHT ) {
                if ( dy < 0 ) {
                    dy = fabs( dy ) - WALL_SEP;
                } else {
                    dy = -fabs( dy ) + WALL_SEP;
                }
            }

            if ( dx != 0 || dy != 0 ) {
                double r_2 = f_max( dx * dx + dy * dy, min_r2 );

                double atobAngle = fabs( atan2( dy, dx ) - atan2( a.vy, a.vx ) );
                double btoaAngle = fabs( atan2( -dy, -dx ) - atan2( b.vy, b.vx ) );

                //adjust a if it can see b
                if (( atobAngle <= ( SIGHT_ANGLE / 2.0 ) )
                        || ( atobAngle >= ( 2*M_PI - SIGHT_ANGLE / 2.0 ) ) ) {
                    // calculate flocking vector
                    if ( r_2 < R_SIGHT2 ) {
                        a.v2x += b.vx;
                        a.v2y += b.vy;

                        a.cogx += b.x;
                        a.cogy += b.y;

                        a.in_sight++;

                        a.fishInteractedWith++;
                        // calculate crowding vector
                        if ( r_2 < R_CROWD2 ) {
                            a.v0x -= dx;
                            a.v0y -= dy;
                            a.in_crowd++;
                        }
                    }
                }

                //adjust b if it can see a
                if (( btoaAngle <= ( SIGHT_ANGLE / 2.0 ) )
                        || ( btoaAngle >= ( 2*M_PI - SIGHT_ANGLE / 2.0 ) ) ) {
                    // calculate flocking vector
                    if ( r_2 < R_SIGHT2 ) {
                        b.v2x += a.vx;
                        b.v2y += a.vy;

                        b.cogx += a.x;
                        b.cogy += a.y;

                        b.in_sight++;

                        b.fishInteractedWith++;
                        // calculate crowding vector
                        if ( r_2 < R_CROWD2 ) {
                            b.v0x += dx;
                            b.v0y += dy;
                            b.in_crowd++;
                        }
                    }
                }
            }
        }
    }
}

void LeafNode::interQuadInteract()
{
    for ( FishList::iterator i = contents.begin(); i != contents.end();i++ ) {
        /*
         * find all leaf node quads that may have fish close enough to this one
         * and interact them
         */
        Root->interact( **i, this );
    }
}

void LeafNode::interact( fish_t &f, const LeafNode * const originator )
{
    /*
     * don't interact if I originated the call
     * check if one of my corners is within the fish's cutoff
     * if so, interact all fish in this quad with f
     */
    if ( this == originator ) {
        return ;
    }
    bool overlap = false;
    overlap |= distance2( f.x, f.y, left, top ) <= CUTOFF2;
    overlap |= distance2( f.x, f.y, right, top ) <= CUTOFF2;
    overlap |= distance2( f.x, f.y, left, bottom ) <= CUTOFF2;
    overlap |= distance2( f.x, f.y, right, bottom ) <= CUTOFF2;
    if ( overlap ) {
        for ( FishList::iterator i = contents.begin(); i != contents.end();i++ ) {
            fish_t &b = **i;
            double dx = b.x - f.x;
            if ( fabs( dx ) > WALL_SEP - R_SIGHT ) {
                if ( dx < 0 ) {
                    dx = fabs( dx ) - WALL_SEP;
                } else {
                    dx = -fabs( dx ) + WALL_SEP;
                }
            }
            double dy = b.y - f.y;
            if ( fabs( dy ) > WALL_SEP - R_SIGHT ) {
                if ( dy < 0 ) {
                    dy = fabs( dy ) - WALL_SEP;
                } else {
                    dy = -fabs( dy ) + WALL_SEP;
                }
            }

            if ( dx != 0 || dy != 0 ) {
                double r_2 = f_max( dx * dx + dy * dy, min_r2 );

                // figure out if the boid is in this boid's range of vision
                double angle = fabs( atan2( dy, dx ) - atan2( f.vy, f.vx ) );

                if (( angle <= ( SIGHT_ANGLE / 2.0 ) ) || ( angle >= ( 2*M_PI - SIGHT_ANGLE / 2.0 ) ) ) {
                    // calculate flocking vector
                    if ( r_2 < R_SIGHT2 ) {
                        f.v2x += b.vx;
                        f.v2y += b.vy;

                        f.cogx += b.x;
                        f.cogy += b.y;

                        f.in_sight++;

                        f.fishInteractedWith++;

                        // calculate crowding vector
                        if ( r_2 < R_CROWD2 ) {
                            f.v0x -= dx;
                            f.v0y -= dy;
                            f.in_crowd++;
                        }
                    }
                }
            }
        }
    }
}

bool LeafNode::findHome( fish_t &fish, LeafNode *&newNode, int &newThread )
{
    if ( !belongsHere( fish ) ) {
        return false;
    } else {
        newNode = this;
        newThread = owner;
        return true;
    }
}

bool LeafNode::belongsHere( fish_t &f )
{
    switch ( whichChild ) {
    case 0:
        return ( f.x >= left ) && ( f.x <= right ) && ( f.y >= top ) && ( f.y <= bottom );
        break;
    case 1:
        return ( f.x > left ) && ( f.x <= right ) && ( f.y >= top ) && ( f.y <= bottom );
        break;
    case 2:
        return ( f.x >= left ) && ( f.x <= right ) && ( f.y > top ) && ( f.y <= bottom );
        break;
    case 3:
        return ( f.x > left ) && ( f.x <= right ) && ( f.y > top ) && ( f.y <= bottom );
        break;
    default:
      assert(0);
      return false;
      break;
    }
}

void LeafNode::insert( fish_t *fish )
{
    contents.push_back( fish );
}

void LeafNode::splitUp( FishThread &thread )
{
    /*
    * if I'm full, split me up
    * What if all the fish have the same position? we would recursively splitUp forever
    * and split up quadtrees pointlessly (the quad that started the recursion would contain
    * all the fish)
    * To prevent that, we make sure that the first and last fish are not on top of each other
    * This results in at most (MAX_ITEMS - 1)
    * more fish in a quad than there should be, until a different position fish
    * is inserted and the fish that are different than fish[0] are put into new quads.
    * (worst case scenario is fish[0], followed by (MAX_ITEMS-1) fish different from fish[0]
    * followed by a bunch of fish identical to fish[0].
    * The alternative is to compare the fish after front() to front(), and split up if they're different
    */

    if ( contents.size() > MaxFish
            && ( contents.front()->x != contents.back()->x || contents.front()->y != contents.back()->y ) ) {
        numSplits++;
        TreeNode *replacement = new TreeNode( parent, whichChild, owner, left, right, top, bottom );
        for ( int i = 0;i < 4;i++ ) {
            //I know they're leafnodes because I just constructed the treenode
            thread.addLeaf( dynamic_cast<LeafNode*>( replacement->children[i] ) );
        }
        /*
         * if my parent is not owned, then add replacement to the list of root nodes,
         * because if my parent is owned, then it, or its ancestor is a root node already
         */
        if ( parent && parent->owner == -1 ) {
            thread.addRoot( replacement );
        }

        /*
         * insert my fish into the new quadtree
         */
        for ( FishList::iterator i = contents.begin(); i != contents.end();i++ ) {
            replacement->insert( *i );
        }
        //recursive call to ensure that my children are also fully split
        replacement->splitUp( thread );

        parent->children[whichChild] = replacement;

        thread.removeLeaf( this );
        delete this;
    }
}

void LeafNode::stealFish( FishList &addHere, FishThread &thread )
{
    addHere.splice( addHere.end(), contents );
    //remove myself from leaf list because I am about to be deleted
    thread.removeLeaf( this );
}

int LeafNode::updateFishCount()
{
    return contents.size();
}

TreeNode::TreeNode( TreeNode *parent, int whichChild, int owner, double left, double right, double top, double bottom )
        : QuadTree( parent, whichChild, owner, left, right, top, bottom )
{
    horizCenter = ( right - left ) / 2 + left;
    verticalCenter = ( bottom - top ) / 2 + top;

    children[0] = new LeafNode( this, 0, owner,
                                left, horizCenter,
                                top, verticalCenter );
    children[1] = new LeafNode( this, 1, owner,
                                horizCenter, right,
                                top, verticalCenter );
    children[2] = new LeafNode( this, 2, owner,
                                left, horizCenter,
                                verticalCenter, bottom );
    children[3] = new LeafNode( this, 3, owner,
                                horizCenter, right,
                                verticalCenter, bottom );
}

TreeNode::~TreeNode()
{
    for ( int i = 0;i < 4;i++ ) {
        delete children[i];
    }
}

void TreeNode::insert( fish_t *fish )
{
    /*
     * basically, quadrant 0 includes all its sides
     * quad 1 doesn't include left side
     * quad 2 doesn't include top
     * quad 3 doesn't include left or top
     */
    int quad;
    if ( fish->x <= horizCenter ) {
        if ( fish->y <= verticalCenter ) {
            quad = 0;
        } else {
            quad = 2;
        }
    } else {
        if ( fish->y <= verticalCenter ) {
            quad = 1;
        } else {
            quad = 3;
        }
    }
    children[quad]->insert( fish );

    numChildren++;
}

void TreeNode::splitUp( FishThread &thread )
{
    for ( int i = 0;i < 4;i++ ) {
        children[i]->splitUp( thread );
    }
}

void TreeNode::cleanupRoots( FishThread &thread )
{
    if ( parent ) {
        if ( parent->owner == owner ) {
            thread.removeRoot( this );
        } else {
            //my parent should not have a different owner than me (if its owned at all)
            assert( parent->owner == -1 );
        }
    }
}

void TreeNode::stealFish( FishList &lst, FishThread &thread )
{
    /*
     * steal my children's fish
     * someone is also stealing my fish, which means they will delete me,
     * and my destructor will delete my children
     */
    for ( int i = 0;i < 4;i++ ) {
        children[i]->stealFish( lst, thread );
    }
}

void TreeNode::collapse( FishThread &thread )
{
    /*
     * if I'm root node (no parent) then don't collapse me
     * if total # of fish underneath me is less than MinFish, steal all my children's fish,
     * make a new leafnode to replace me, and replace me, remove me
     * from root list and add new leafnode to leaf list
     * if not, call collapse on my children
     */
    if ( nFishOwned < MinFish && parent ) {
        numCollapses++;
        LeafNode *replacement = new LeafNode( parent, whichChild, owner, left, right, top, bottom );
        for ( int i = 0; i < 4; ++i ) {
            children[i]->stealFish( replacement->contents, thread );
        }
        thread.removeRoot( this );
        thread.addLeaf( replacement );
        parent->children[whichChild] = replacement;
        delete this;
    } else {
        for ( int i = 0; i < 4; ++i ) {
            children[i]->collapse( thread );
        }
    }
}

int TreeNode::updateFishCount()
{
    nFishOwned = 0;
    for ( int i = 0; i < 4; ++i ) {
        nFishOwned += children[i]->updateFishCount();
    }
    return nFishOwned;
}

void TreeNode::distributeFish( int &currentThread, int &fishOwned, FishThread threads[] )
{
    //call distributeFish on my 4 children, and then figure out if I'm owned by a thread
    for ( int i = 0;i < 4;i++ ) {
        children[i]->distributeFish( currentThread, fishOwned, threads );
    }
    int zerosOwner = children[0]->owner;
    bool sameOwner = true;
    for ( int i = 1;i < 4;i++ ) {
        sameOwner &= zerosOwner == children[i]->owner;
    }

    if ( sameOwner ) {
        owner = zerosOwner;
        threads[owner].addRoot( this );
    } else {
        //TODO doing a test here with this assert
        assert( owner == -1 );
        owner = -1;
    }
}

void TreeNode::interact( fish_t &f, const LeafNode * const originator )
{
    /*
     * check if the fish is inside me, or if one of my corners
     * is within the fish's cutoff
     * NOTE that insideMe could be wrong if the fish is on a border line
     * but at worst this would cause us to call interact when unnecesssary
     * causing degraded performance but correct operation
     */
    bool insideMe = ( f.x >= left ) && ( f.x <= right ) && ( f.y >= top ) && ( f.y <= bottom );
    bool overlap = false;
    overlap |= distance2( f.x, f.y, left, top ) <= CUTOFF2;
    overlap |= distance2( f.x, f.y, right, top ) <= CUTOFF2;
    overlap |= distance2( f.x, f.y, left, bottom ) <= CUTOFF2;
    overlap |= distance2( f.x, f.y, right, bottom ) <= CUTOFF2;
    if ( insideMe || overlap ) {
        for ( int i = 0;i < 4;i++ ) {
            children[i]->interact( f, originator );
        }
    }
}

bool TreeNode::findHome( fish_t &fish, LeafNode *&newNode, int &newThread )
{
    //TODO i wonder if its faster to try a random child first, or always start from 0
    for ( int i = 0;i < 4;i++ ) {
        bool stop = children[i]->findHome( fish, newNode, newThread );
        if ( stop ) {
            return true;
        }
    }
    return false;
}
